//
// Created by HP on 2024/4/29.
//

#include "Aurora_mode.h"
#include "rtthread.h"


Aurora_Mode::Aurora_Mode(LCD_Driver *lcd,Aurora_Font *font,uint8_t sel_m,uint8_t m_sel){
    _lcd = lcd;
    _font = font;


    select_motor = sel_m;  //左电机
    mode_sel = m_sel;
}

Aurora_Mode::~Aurora_Mode(){

}






void Aurora_Mode::mode_label_init(){
    title_label.set_font_driver(_font);
    title_label.set_lcd_driver(_lcd);
    title_label.set_font_size(Aurora_Font::font_size_big);
    title_label.set_text("模式选择");
    title_label.set_pos(100,0);
    title_label.show();


    obj_label.set_font_driver(_font);
    obj_label.set_lcd_driver(_lcd);
    obj_label.set_font_size(Aurora_Font::font_size_small);
    obj_label.set_color(0x00f8);
    if( select_motor == MODE_LEFT_MOTOR)
        obj_label.set_text("左电机");
    else if( select_motor == MODE_RIGHT_MOTOR)
        obj_label.set_text("右电机");
    else
        obj_label.set_text("左右电机");
    obj_label.set_pos(130,150);
    obj_label.show();



    NONE_label.set_font_driver(_font);
    NONE_label.set_lcd_driver(_lcd);
    NONE_label.set_font_size(Aurora_Font::font_size_middle);
    NONE_label.set_text("暂停");
    NONE_label.set_pos(130,75);
    if( mode_sel == MODE_HALT_MOTOR)
        NONE_label.set_select(ENABLE);
    else
        NONE_label.set_select(DISABLE);
    NONE_label.show();

    OPEN_LOOP_label.set_font_driver(_font);
    OPEN_LOOP_label.set_lcd_driver(_lcd);
    OPEN_LOOP_label.set_font_size(Aurora_Font::font_size_middle);
    OPEN_LOOP_label.set_text("开环");
    OPEN_LOOP_label.set_pos(35,50);
    if( mode_sel == MODE_OPEN_MOTOR)
        OPEN_LOOP_label.set_select(ENABLE);
    else
        OPEN_LOOP_label.set_select(DISABLE);
    OPEN_LOOP_label.show();

    I_LOOP_label.set_font_driver(_font);
    I_LOOP_label.set_lcd_driver(_lcd);
    I_LOOP_label.set_font_size(Aurora_Font::font_size_middle);
    I_LOOP_label.set_text("电流环");
    I_LOOP_label.set_pos(220,50);
    if( mode_sel == MODE_I_MOTOR)
        I_LOOP_label.set_select(ENABLE);
    else
        I_LOOP_label.set_select(DISABLE);
    I_LOOP_label.show();

    SPEED_LOOP_label.set_font_driver(_font);
    SPEED_LOOP_label.set_lcd_driver(_lcd);
    SPEED_LOOP_label.set_font_size(Aurora_Font::font_size_middle);
    SPEED_LOOP_label.set_text("速度环");
    SPEED_LOOP_label.set_pos(35,110);
    if( mode_sel == MODE_SPEED_MOTOR)
        SPEED_LOOP_label.set_select(ENABLE);
    else
        SPEED_LOOP_label.set_select(DISABLE);
    SPEED_LOOP_label.show();

    POSITION_LOOP_label.set_font_driver(_font);
    POSITION_LOOP_label.set_lcd_driver(_lcd);
    POSITION_LOOP_label.set_font_size(Aurora_Font::font_size_middle);
    POSITION_LOOP_label.set_text("位置环");
    POSITION_LOOP_label.set_pos(220,110);
    if( mode_sel == MODE_POSITION_MOTOR)
        POSITION_LOOP_label.set_select(ENABLE);
    else
        POSITION_LOOP_label.set_select(DISABLE);
    POSITION_LOOP_label.show();

}


void Aurora_Mode::switch_sel_motor(){
    _lcd->fillpart(120,145,240,172,0xffff);
    if(select_motor == MODE_LEFT_MOTOR){
        select_motor = MODE_RIGHT_MOTOR;
        obj_label.set_text("右电机");
        obj_label.set_pos(130,150);
        obj_label.show();
    }
    else if( select_motor == MODE_RIGHT_MOTOR){
        select_motor = MODE_BOTH_MOTOR;
        obj_label.set_text("左右电机");
        obj_label.set_pos(120,150);
        obj_label.show();

    }
    else{
        select_motor = MODE_LEFT_MOTOR;
        obj_label.set_text("左电机");
        obj_label.set_pos(130,150);
        obj_label.show();
    }
}


void Aurora_Mode::switch_prev_mode(){
    if( mode_sel == MODE_POSITION_MOTOR){
        SPEED_LOOP_label.set_select(ENABLE);
        SPEED_LOOP_label.show();
        POSITION_LOOP_label.set_select(DISABLE);
        POSITION_LOOP_label.show();

        mode_sel = MODE_SPEED_MOTOR;
    }
    else if( mode_sel == MODE_SPEED_MOTOR){
        I_LOOP_label.set_select(ENABLE);
        I_LOOP_label.show();
        SPEED_LOOP_label.set_select(DISABLE);
        SPEED_LOOP_label.show();

        mode_sel = MODE_I_MOTOR;
    }
    else if( mode_sel == MODE_I_MOTOR ) {
        OPEN_LOOP_label.set_select(ENABLE);
        OPEN_LOOP_label.show();
        I_LOOP_label.set_select(DISABLE);
        I_LOOP_label.show();


        mode_sel = MODE_OPEN_MOTOR;
    }
    else if( mode_sel == MODE_OPEN_MOTOR ) {
        NONE_label.set_select(ENABLE);
        NONE_label.show();
        OPEN_LOOP_label.set_select(DISABLE);
        OPEN_LOOP_label.show();

        mode_sel = MODE_HALT_MOTOR;
    }
    else if( mode_sel == MODE_HALT_MOTOR ){
        POSITION_LOOP_label.set_select(ENABLE);
        POSITION_LOOP_label.show();
        NONE_label.set_select(DISABLE);
        NONE_label.show();
        mode_sel = MODE_POSITION_MOTOR;
    }
    else if(  mode_sel == MODE_Calibration_MOTOR ){
        NONE_label.set_select(ENABLE);
        NONE_label.show();

        OPEN_LOOP_label.set_select(DISABLE);
        OPEN_LOOP_label.show();

        I_LOOP_label.set_select(DISABLE);
        I_LOOP_label.show();

        SPEED_LOOP_label.set_select(DISABLE);
        SPEED_LOOP_label.show();

        POSITION_LOOP_label.set_select(DISABLE);
        POSITION_LOOP_label.show();

        mode_sel = MODE_HALT_MOTOR;
    }
}
void Aurora_Mode::switch_next_mode(){
    if( mode_sel == MODE_POSITION_MOTOR){
        NONE_label.set_select(ENABLE);
        NONE_label.show();
        POSITION_LOOP_label.set_select(DISABLE);
        POSITION_LOOP_label.show();

        mode_sel = MODE_HALT_MOTOR;
    }
    else if( mode_sel == MODE_HALT_MOTOR){
        OPEN_LOOP_label.set_select(ENABLE);
        OPEN_LOOP_label.show();
        NONE_label.set_select(DISABLE);
        NONE_label.show();

        mode_sel = MODE_OPEN_MOTOR;
    }
    else if( mode_sel == MODE_OPEN_MOTOR ) {
        I_LOOP_label.set_select(ENABLE);
        I_LOOP_label.show();
        OPEN_LOOP_label.set_select(DISABLE);
        OPEN_LOOP_label.show();

        mode_sel = MODE_I_MOTOR;
    }
    else if( mode_sel == MODE_I_MOTOR ) {
        SPEED_LOOP_label.set_select(ENABLE);
        SPEED_LOOP_label.show();
        I_LOOP_label.set_select(DISABLE);
        I_LOOP_label.show();

        mode_sel = MODE_SPEED_MOTOR;
    }
    else if( mode_sel == MODE_SPEED_MOTOR ){
        POSITION_LOOP_label.set_select(ENABLE);
        POSITION_LOOP_label.show();
        SPEED_LOOP_label.set_select(DISABLE);
        SPEED_LOOP_label.show();
        mode_sel = MODE_POSITION_MOTOR;
    }
    else if(  mode_sel == MODE_Calibration_MOTOR ){
        NONE_label.set_select(ENABLE);
        NONE_label.show();

        OPEN_LOOP_label.set_select(DISABLE);
        OPEN_LOOP_label.show();

        I_LOOP_label.set_select(DISABLE);
        I_LOOP_label.show();

        SPEED_LOOP_label.set_select(DISABLE);
        SPEED_LOOP_label.show();

        POSITION_LOOP_label.set_select(DISABLE);
        POSITION_LOOP_label.show();

        mode_sel = MODE_HALT_MOTOR;
    }
}

void Aurora_Mode::calibration_motor(){
    mode_sel = MODE_Calibration_MOTOR;
}

void Aurora_Mode::send_FOC_mode() {
    if (select_motor == MODE_LEFT_MOTOR)
        ui2foc_msg.type = enable_mode_Lmotor;
    else if (select_motor == MODE_RIGHT_MOTOR)
        ui2foc_msg.type = enable_mode_Rmotor;
    else
        ui2foc_msg.type = enable_mode_LRmotor;

    if (mode_sel == MODE_Calibration_MOTOR)
        ui2foc_msg.foc_mode = Calibration_MOTOR;
    else if (mode_sel == MODE_HALT_MOTOR)
        ui2foc_msg.foc_mode = HALT_MOTOR;
    else if (mode_sel == MODE_OPEN_MOTOR)
        ui2foc_msg.foc_mode = OPEN_LOOP_MOTOR;
    else if (mode_sel == MODE_I_MOTOR)
        ui2foc_msg.foc_mode = I_LOOP_MOTOR;
    else if (mode_sel == MODE_SPEED_MOTOR)
        ui2foc_msg.foc_mode = SPEED_LOOP_MOTOR;
    else if (mode_sel == MODE_POSITION_MOTOR)
        ui2foc_msg.foc_mode = POSITION_LOOP_MOTOR;
    ui2foc_msg.cmd = IS_CMD;
}

void Aurora_Mode::send_HALT_motor(){
    ui2foc_msg.type = enable_mode_LRmotor;
    ui2foc_msg.foc_mode = HALT_MOTOR;
    ui2foc_msg.cmd = IS_CMD;
}